#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <errno.h>
#include <fcntl.h>
#include <string.h>
#include <stdio.h>
#include <sys/time.h>   //      gettimeofday
#include <sys/stat.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netdb.h>
#include <errno.h>
#include <math.h>


#include "p2v.h"
#include "typedef.h"
#include "udp_get_bitrate.h"
#include "serial.h"
#include "uartinf.h"
#include "matrix.h"


extern void LCDDisplayASCII8X16(u32 x0, u32 y0, u32 ForeColor, char * s);
extern u32 g_pLCDBuffer;

uint16_t x_res = 1920;
uint16_t y_res = 1080;

float rssi;
float rssi1;
float rssi2;


uint32_t *working_fb;

int *switch_mode;
int pre_switch_mode;
int switch_mode_pre=0;

#define RED     (0xFFFF0000)
#define BLUE    (0xFF0000FF)
#define GREEN   (0xFF00FF00)
#define YELLOW  (0xFFFFFF00)
#define BLACK   (0xFF000000)


struct display_param{
          int FREQ;
	  int BW;
	  int POWER;
	  int bitrate;
          float RSSI;
          float RSSI1;
          float RSSI2;
};

int *switch_memory_open(void) {
        char buf[128];
        int fd;
        sprintf(buf, "/switch_mode");
        fd = shm_open(buf, O_CREAT | O_RDWR, S_IRUSR | S_IWUSR);
        if(fd < 0) { perror("shm_open"); exit(1); }
        if (ftruncate(fd, 4096) == -1) { perror("ftruncate"); exit(1); }
        int *retval = mmap(NULL, 4096, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0);
        return retval;
}

static void fill_bmp(int fd, uint32_t *buf, uint32_t alpha, int width, int height)
{
    int i, j;
    unsigned char tmp_buf[54];
    unsigned char *bmp_line;

    uint32_t *p = buf;

    printf("++fill_bmp\n");
    unsigned char *bmp_buf  = (unsigned char *)malloc(width*height*3);
    if(bmp_buf == NULL)
        return;

    read(fd, tmp_buf, 54); //skip bmp hdr, TODO,parse hdr
    i = read(fd, bmp_buf, width*height*3);
    if(i != width*height*3) {
        printf("read bmp file fail\n");
        return;
    }
    for(i=0; i<height; i++) {
        bmp_line = bmp_buf + ((height-1-i) * width * 3);
        for(j=0;j<width;j++) {
               *p = (alpha<<24) | (bmp_line[j*3]) |  (bmp_line[j*3+1]<<8) |  ((bmp_line[j*3+2]<<16));
           p++;
        }
    }
    printf("--fill_bmp, %d, %d\n", i, j);
    free(bmp_buf);
}

void SetPixelOS(int16_t x, int16_t y, uint32_t color) //set pixel off screen
{
  working_fb[y * x_res + x] = color;
}


void DrawLineOS(int16_t x1, int16_t y1, int16_t x2, int16_t y2, uint32_t color)
{
  int16_t temp;
  int dalt_x,dalt_y,err=0;
 if (y1>y2)
    {
    temp=x1;
    x1=x2;
    x2=temp;
    temp=y1;
    y1=y2;
    y2=temp;
    }
SetPixelOS(x1,y1,color);
 dalt_x=x2-x1;
 dalt_y=y2-y1;
 if(dalt_x>=0)
     {
      if(dalt_y>dalt_x)//k>1
        {
         while(y1<y2)
         {
          if(err<0)
          {
           x1=x1+1;
           y1=y1+1;
           err=err+dalt_y-dalt_x;
           }
           else
           {
           y1=y1+1;
           err=err-dalt_x;
           }
          SetPixelOS(x1,y1,color);
         }
       }
     else  // 0<=k=<1
      {
       if (dalt_y==0)
          y1=y1-1;
       while(x1<x2)
         {
          if(err<0)
          {
                x1=x1+1;
           err=err+dalt_y;
           }
           else
           {
           y1=y1+1;
                x1=x1+1;
           err=err+dalt_y-dalt_x;
           }
          SetPixelOS(x1,y1,color);
         }
     }

  }
else
  {
   dalt_x=x1-x2;
    if(dalt_y>dalt_x)//k<-1
     {
      while(y1<y2)
       {
        if(err<0)
         {
          x1=x1-1;
          y1=y1+1;
          err=err+dalt_y-dalt_x;
         }
        else
         {
          y1=y1+1;
          err=err-dalt_x;
         }
        SetPixelOS(x1,y1,color);
      }
    }
   else       //0>k>=-1
    {
      if (dalt_y==0)
          y1=y1-1;
    while(x1>x2)
      {
       if(err<0)
        {
         x1=x1-1;
         err=err+dalt_y;
        }
              else
        {
         x1=x1-1;
         y1=y1+1;
         err=err+dalt_y-dalt_x;
        }
       SetPixelOS(x1,y1,color);
      }
    }
  }

}


void Rect(uint16_t x0, uint16_t y0, uint16_t w, uint16_t h,unsigned int color){
        int i=0;

        for(i=0;i<h;i++){
                DrawLineOS(x0,y0+i,x0+w,y0+i,color);

        }


}

void Rect_storll(uint16_t x0, uint16_t y0,uint16_t x1, uint16_t y1,uint16_t w,unsigned int color){
        int i;

        for(i=0;i<w;i++){
                DrawLineOS(x0+i,y0,x1+i,y1,color);
        }


}

void yuanhu(uint16_t x0, uint16_t y0, int r,double start_hudu,double end_hudu,unsigned int color,int y_limiter){

	double hudu;
	int i;
	uint16_t x;
	uint16_t y;

	for (hudu=start_hudu;hudu<end_hudu;hudu+=0.01){
		x=x0+r*cos(hudu*M_PI/180);
		y=y0+r*sin(hudu*M_PI/180);
		if(y>= y_limiter){
//		for(i=0;i<w;i++){
		SetPixelOS(x,y,color);
//		}

		}
	}



}





void blt_clr(uint32_t *fb, uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1)
{
        uint32_t *src_addr;
        int x_stride;
        int i;
        int iter = y1 - y0 + 1;

        x_stride = (x1 - x0 + 1) * 4;
        for(i=0; i<iter; i++) {
                src_addr = &fb[(y0 + i)*x_res + x0];
                memset(src_addr, 0x0, x_stride);
        }
}



#define BG_COLOR 0xff0000ff
void blt_bg(uint32_t *fb, uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1)
{
        uint32_t *src_addr;
        int x_stride;
        int i;
        int j;
        int iter = y1 - y0 + 1;

        x_stride = (x1 - x0 + 1);
        for(i=0; i<iter; i++) {
                src_addr = &fb[(y0 + i)*x_res + x0];
                //memset(src_addr, 0x0, x_stride);
                for(j = 0; j < x_stride; j++)
                        src_addr[j] = BG_COLOR;
        }
}

void blt_bg_white(uint32_t *fb, uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1)
{
        uint32_t *src_addr;
        int x_stride;
        int i;
        int j;
        int iter = y1 - y0 + 1;

        x_stride = (x1 - x0 + 1);
        for(i=0; i<iter; i++) {
                src_addr = &fb[(y0 + i)*x_res + x0];
                //memset(src_addr, 0x0, x_stride);
                for(j = 0; j < x_stride; j++)
                        src_addr[j] = 0xff000000;
        }
}


int calc_radius(double C14){
	int a=3450;
	int b=1660;
	int c=2150;
	int d=600;
	double value1;
	double value2;

	value1 = ((b/cos(C14/180*M_PI)+a-b)/cos((90-C14)/180*M_PI)) ;
	value2 = b*tan(C14/180*M_PI) ;

	
	return (int)(value1-value2 + c/2 + d/2);

	//return rad;

}




int render(double w0,int x0,int y0,int right){
	int RADIUS;
	int G_RADIUS;
	int x_center;
	int y_center;
	int y_limiter;
	int x_limiter;
	double w1;
	int i;	
	y_limiter = y0-120;
	int x_array[3];
	int y_array[3];

	y_array[0] =  y0-120;
	y_array[1] =  y0-60 ;
	y_array[2] =  y0-20 ;


	if(w0>0){
		RADIUS = calc_radius(w0);
		G_RADIUS = (RADIUS *130 )/2150;		
		x_center = x0-G_RADIUS*cos(w0/180 * M_PI);
		y_center = y0+G_RADIUS*sin(w0/180 * M_PI);
		for(i=0;i<3;i++)
			x_array[i] = sqrt(G_RADIUS*G_RADIUS - (y_center-y_array[i])*(y_center-y_array[i]))+x_center;

		//printf(" %d %d %d %d %d\n",G_RADIUS , x_center,y_center,x_limiter,y_limiter);
		if(right){	
			Rect(x_array[0]-80,y_array[0],80,5,GREEN);
                	Rect(x_array[1]-60,y_array[1],60,5,YELLOW);
                	Rect(x_array[2]-40,y_array[2],40,5,RED);
		}else{
			Rect(x_array[0],y_array[0],80,5,GREEN);
                        Rect(x_array[1],y_array[1],60,5,YELLOW);
                        Rect(x_array[2],y_array[2],40,5,RED);

		}
	

		for(i=0;i<10;i++)			
			yuanhu(x_center,y_center,G_RADIUS-i,270,360-w0,GREEN,y_limiter);

	}else if(w0 < 0){
		RADIUS = calc_radius(abs(w0));
		G_RADIUS = (RADIUS *130 )/2150;
		x_center = x0+G_RADIUS*cos(-w0/180 * M_PI);
		y_center = y0+G_RADIUS*sin(-w0/180 * M_PI);
		for(i=0;i<3;i++)
			x_array[i] = x_center-sqrt(G_RADIUS*G_RADIUS - (y_center-y_array[i])*(y_center-y_array[i]));
		//printf(" %d %d %d %d %d\n",G_RADIUS , x_center,y_center,x_limiter,y_limiter);
	
		 if(right){
                        Rect(x_array[0]-80,y_array[0],80,5,GREEN);
                        Rect(x_array[1]-60,y_array[1],60,5,YELLOW);
                        Rect(x_array[2]-40,y_array[2],40,5,RED);
                }else{
                        Rect(x_array[0],y_array[0],80,5,GREEN);
                        Rect(x_array[1],y_array[1],60,5,YELLOW);
                        Rect(x_array[2],y_array[2],40,5,RED);

                }
	
		for(i=0;i<10;i++)
			yuanhu(x_center,y_center,G_RADIUS-i,180-w0,270,GREEN,y_limiter);

	}else{

	

	



	
		if(right){
			Rect(x0-40-60,y0-120,60,5,GREEN);
			Rect(x0-60,y0-60,40,5, YELLOW);
			Rect(x0-30,y0-20,20,5, RED);

			Rect_storll(x0,y0,x0-40,y0-120,10,GREEN);

		}else{
			Rect(x0+50,y0-120,60,5,GREEN);
                        Rect(x0+30,y0-60,40,5, YELLOW);
                        Rect(x0+20,y0-20,20,5, RED);

                        Rect_storll(x0,y0,x0+40,y0-120,10,GREEN);


	

		}

		
		
		

	}

	


}




int num = 220;

double scale = 0.01;

//基本参数
double a1 = 3450, a2 = 3450;
double b1 = 1660, b2 = 1790;
double c1 = 2150, c2 = 2150;
double d1 = 600, d2 = 600;
// double e = 3000, f = 2976, g = 510, h = 2200;
// double e = 0, f = 2976, g = 510, h = 2200;
double e = 3800, f = 2976, g = 510, h = 2700;
double fw = 2350, hw = 1200, w = 1800;
//相机水平偏移量,视频中车子中轴向左则为正
double cz = -250;
//最大正值角度
double maxAngle = 36;

//相机参数
// double z = 12;
double z = 15;
// double FOV0 = 77.17;
// double FOV1 = 77.17;
double FOV0 = 94;
double FOV1 = 94;
double width = 1920, height = 1080;
double near = 0.3;
double far = 1000;
double focalLenght = 1.66;

//屏幕参数
double offsetX = 640, offsetY = 360, offsetScale = 3;
// double offsetX = -1000, offsetY = 0, offsetScale = 1;
double viewMinY = 360, viewMaxY = 720;
double viewMinX = 640, viewMaxX = 1280;
// double viewMinY = 0, viewMaxY = 1080;

//畸变参数
int isDistortion = 0;   //是否计算畸变
double k1 = -0.28340811;
double k2 = 0.07395907;
double k3 = 0;
double p1 = 0.00019359;
double p2 = 1.76187114e-05;
double s1 = 0;
double s2 = 0;

Matrix *matrix0;
Matrix *matrix1;
Matrix *matrix2;
Matrix *matrix3;
Matrix *matrix4;
Matrix *matrix5;
Matrix *matrix6;

void SetVectorPixelOS(Vector *vec, uint32_t color) 
{
        int blockLen = 4;
        for (int i=-blockLen/2;i<blockLen/2;i++)
        {
                for (int j=-blockLen/2;j<blockLen/2;j++)
                {
                        SetPixelOS((int16_t)(vec->data[0]+i), (int16_t)(vec->data[1]+j), color);
                }
        }
}

double cot(double radian)
{
        return 1/tan(radian);
}

double deg2Rad(double angle) {
        return angle / 180 * M_PI;
}

void InitParameter()
{
        if (scale != 0)
        {
                a1 *= scale;
                a2 *= scale;
                b1 *= scale;
                b2 *= scale;
                c1 *= scale;
                c2 *= scale;
                d1 *= scale;
                d2 *= scale;
                e *= scale;
                f *= scale;
                g *= scale;
                h *= scale;
                cz *= scale;
                fw *= scale;
                hw *= scale;
                w *= scale;
        }
        double value0[] = {
                1,0,0,0,
                0,1,0,-h,
                0,0,1,-cz,
                0,0,0,1
                };
        double val90 = deg2Rad(90);
        double cosVal90 = cos(val90);
        double sinVal90 = sin(val90);
        double value1[] = {
                cosVal90,0,sinVal90,0,
                0,1,0,0,
                -sinVal90,0,cosVal90,0,
                0,0,0,1
                };
        double valz = deg2Rad(-z);
        double cosValz = cos(valz);
        double sinValz = sin(valz);
        double value2[] = {
                1,0,0,0,
                0,cosValz,-sinValz,0,
                0,sinValz,cosValz,0,
                0,0,0,1
                };
        double cotValFOV0 = cot(deg2Rad(FOV0/2));
        double cotValFOV1 = cot(deg2Rad(FOV1/2));
        double aspect = width/height;
        double value3[] = {
                cotValFOV0/aspect,0,0,0,
                0,cotValFOV1,0,0,
                0,0,-(far+near)/(far-near),-(2*near*far)/(far-near),
                0,0,-1,0
                };
        
        matrix0 = InitMatrix(matrix0,4,4);
        matrix1 = InitMatrix(matrix1,4,4);
        matrix2 = InitMatrix(matrix2,4,4);
        matrix3 = InitMatrix(matrix3,4,4);

        ValueMatrix(matrix0,value0);
        ValueMatrix(matrix1,value1);
        ValueMatrix(matrix2,value2);
        ValueMatrix(matrix3,value3);
}

void InitParamMatrix(double w0)
{
        double value4[] = {
                1,0,0,-g,
                0,1,0,0,
                0,0,1,0,
                0,0,0,1
                };
        double valY = deg2Rad(-w0);
        double cosValY = cos(valY);
        double sinValY = sin(valY);
        double value5[] = {
                cosValY,0,sinValY,0,
                0,1,0,0,
                -sinValY,0,cosValY,0,
                0,0,0,1
                };
        double value6[] = {
                1,0,0,g,
                0,1,0,0,
                0,0,1,0,
                0,0,0,1
                };

        matrix4 = InitMatrix(matrix4,4,4);
        matrix5 = InitMatrix(matrix5,4,4);
        matrix6 = InitMatrix(matrix6,4,4);

        ValueMatrix(matrix4,value4);
        ValueMatrix(matrix5,value5);
        ValueMatrix(matrix6,value6);
}

void FreeParameter()
{
        FreeMatrix(matrix0);
        FreeMatrix(matrix1);
        FreeMatrix(matrix2);
        FreeMatrix(matrix3);
}

void FreeParamMatrix()
{
        FreeMatrix(matrix4);
        FreeMatrix(matrix5);
        FreeMatrix(matrix6);
}

// //前轮外侧转弯半径(R前外)
// double GetR1(double angle) {
//        return  (b1 / cos(deg2Rad(angle)) + a1 - b1) / cos(deg2Rad(90 - angle)) - b1 * tan(deg2Rad(angle)) + c1 / 2 + d1 / 2;
// }

//前轴中心轨迹半径(R前中)
double GetR1(double angle) {
       return  (b1 / cos(deg2Rad(angle)) + a1 - b1) / cos(deg2Rad(90 - angle)) - b1 * tan(deg2Rad(angle));
}

//后轴中心轨迹半径(R后中)
double GetR2(double angle) {
       return  (b2 / cos(deg2Rad(angle)) + a2 - b2) / cos(deg2Rad(90 - angle)) - b2 * tan(deg2Rad(angle));
}

//前铲外侧轨迹方程
double GetQianchanY(double angle, double x)
{
        if (angle == 0)
        {
                return f / 2;
        }
        else
        {
        // return sqrt(pow((fabs(f / 2 - c1 / 2 - d1 / 2) + GetR1(angle)), 2) + pow(e, 2) - pow((x - (g + b2)), 2)) - GetR2(angle);
                return sqrt(pow((f / 2 + GetR1(angle)), 2) + pow(e, 2) - pow((x - (g + b2)), 2)) - GetR2(angle);
                // return sqrt(pow(GetR1(angle) + c1 / 2 + d1 / 2, 2) - pow((x - (g + b2)), 2)) - GetR2(angle);
        }
}

//前轮内侧转弯轨迹方程
double GetQianlunneiceY(double angle, double x)
{
        if (angle == 0)
        {
                return -c1/ 2 - d1 / 2;
        }
        else
        {
                return sqrt(pow(GetR1(angle) - c1 / 2 - d1 / 2, 2) - pow((x - (g + b2)), 2)) - GetR2(angle);
        }
       
}

int IsScreenPoint(Vector *vector)
{
        if (vector->data[0]<viewMinX || vector->data[1]<viewMinY || vector->data[0]>viewMaxX || vector->data[1]>viewMaxY)
        {
                return 0;
        }
        return 1;
}

int IsClipPoint(Vector *vector)
{
        if ((-vector->data[3] <= vector->data[0] && vector->data[0] <= vector->data[3]) &&
             (-vector->data[3] <= vector->data[1] && vector->data[1] <= vector->data[3]) &&
             (-vector->data[3] <= vector->data[2] && vector->data[2] <= vector->data[3]))
        {
                return 0;
        }
        return 1;
}

//矩阵变换:从世界坐标系(World)到相机坐标系(Camera)
//w齐次坐标:方向补0点补1
Vector *MatrixTransformWToC(Vector *value)
{
        return MultiplyPoint(matrix2, MultiplyPoint(matrix1, MultiplyPoint(matrix0, value)));
}

//矩阵变换:从相机坐标系(Camera)到相机裁剪坐标系(Clip)
Vector *MatrixTransformCToC(Vector *value)
{
        return MultiplyPoint(matrix3, value);
}

//矩阵变换:从相机裁剪坐标系(Clip)到屏幕坐标系(Screen)
Vector *MatrixTransformCToS(Vector *value)
{
        Vector *vecScreen = InitVector(vecScreen, 4);

        vecScreen->data[0] = width - (value->data[0]*width/(2*value->data[3]) + (width/2));
        vecScreen->data[1] = value->data[1]*height/(2*value->data[3]) + (height/2);

        vecScreen->data[0] = vecScreen->data[0] / offsetScale;
        vecScreen->data[1] = vecScreen->data[1] / offsetScale;
        vecScreen->data[0] += offsetX;
        vecScreen->data[1] += offsetY;
        return vecScreen;
}
    
//摄像机畸变变换
Vector *DistortionTransform(Vector *vc)
{
        double xx = vc->data[0] / vc->data[3];
        double yy = vc->data[1] / vc->data[3];
        double r2 = xx * xx + yy * yy;
        Vector *distortion = InitVector(distortion, 4);
        distortion->data[0] = xx * (1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2 * r2) + 2 * p1 * xx * yy + p2 * (r2 + 2 * xx * xx) + s1 * r2;
        distortion->data[1] = yy * (1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2 * r2) + 2 * p2 * xx * yy + p1 * (r2 + 2 * yy * yy) + s2 * r2;
        distortion->data[0] *= vc->data[3];
        distortion->data[1] *= vc->data[3];
        distortion->data[2] = vc->data[2];
        distortion->data[3] = vc->data[3];
        return distortion;
}

//矩阵变换:从世界坐标到屏幕
Vector *MatrixTransform(Vector *value)
{
        double screenValue[] = {-1, viewMinY-1, 0, 0};
        Vector *vecScreen = InitVector(vecScreen, 4);
        ValueVector(vecScreen, screenValue);

        Vector *vecWToCam = MatrixTransformWToC(value);

        Vector *vecCamToClip = MatrixTransformCToC(vecWToCam);

        if (IsClipPoint(vecCamToClip))
         {
                if (isDistortion)
                {
                        Vector *distortion = DistortionTransform(vecCamToClip);
                        vecScreen = MatrixTransformCToS(distortion);
                        FreeVector(distortion);
                }
                else
                {
                        vecScreen = MatrixTransformCToS(vecCamToClip);
                }
        }
                        
        FreeVector(vecCamToClip);
        FreeVector(vecWToCam);

        return vecScreen;
}

void DrawTrajectory(double angle)
{
        int isMirror = 1;
        if (angle < 0)
        {
                isMirror = -1;
                angle =fabs(angle);
        }
        for (int i=-num/2;i<num/2;i++)
        {
                // int i=-14;
                double qianChanValue = isMirror * GetQianchanY(angle, i);
                //判断轨迹点是否在圆内或者圆上，如果在圆外则会返回nan
                if (!isnan(qianChanValue))
                {
                        double vecValue[] = {i, 0, qianChanValue, 1};
                        Vector *vector = InitVector(vector, 4);
                        ValueVector(vector, vecValue);
                        // printf("qianChanValue %d: %lf\n", i, qianChanValue);
                        Vector *vecScreen = MatrixTransform(vector);
                        if (IsScreenPoint(vecScreen))
                        {
                                SetVectorPixelOS(vecScreen, GREEN);
                        }
                        FreeVector(vector);
                        FreeVector(vecScreen);
                }
                
                double qianlunneiceValue = isMirror * GetQianlunneiceY(angle, i);
                if (!isnan(qianlunneiceValue))
                {
                        double vecValue[] = {i, 0, qianlunneiceValue, 1};
                        Vector *vector = InitVector(vector, 4);
                        ValueVector(vector, vecValue);
                        // printf("qianlunneiceValue %d: %lf\n", i, qianlunneiceValue);
                        Vector *vecScreen = MatrixTransform(vector);
                        if (IsScreenPoint(vecScreen))
                        {
                                SetVectorPixelOS(vecScreen, GREEN);
                        }
                        FreeVector(vector);
                        FreeVector(vecScreen);
                }
        }
}

//矩阵变换:横线绕世界坐标系下Y轴旋转
Vector *MatrixTransformHorizontalLine(Vector *vector, double Y)
{
        return MultiplyPoint(matrix6, MultiplyPoint(matrix5, MultiplyPoint(matrix4, vector)));
}

//求直线和圆的交点,取小的值
//z=kx+c
//r*r=(x-a)*(x-a) + (z-b)*(z-b)
Vector *GetPointLineAndCircle(double k, double c, double a, double b, double r)
{
        double delta = (2*(c - b)*k - 2*a)*(2*(c - b)*k - 2*a) - 4*(1 + k*k)*(a*a+(c-b)*(c-b)-r*r);
        double x = ((2*a - 2*(c - b)*k) - sqrt(delta))/(2*(1+k*k));
        double z = k*x+c;
         double vecValue[] = {x, 0, z, 1};
        Vector *vector = InitVector(vector, 4);
        ValueVector(vector, vecValue);
        return vector;
}

//修正画横线的斜率
//经测试,大概在0度时,斜率的总改变量为4;36度时总改变量为0.6,按线性计算总改变量
double GetOffsetK(double angle)
{
        //-36~36
        // angle > 0;offsetK小于零并变小,大于-0.5
        // angle < 0;offsetK大于零并变大,小于0.5
        double k = (0.6f  - 4) / (36 - 0);
        double c = 0.6f - 36 * k;
        // float offsetK = (-angle / parameter.param.maxAngle) * (parameter.param.maxOffsetK);
        //当前度数与最大度数的比值乘以总改变量
        double offsetK = (-angle / maxAngle) * (k * angle + c);
        return offsetK;
}

void DrawHorizontalLine(double angle)
{
        int isMirror = 1;
        if (angle < 0)
        {
                isMirror = -1;
                angle =fabs(angle);
        }
        int xValues[] = {-e-(b1-g),  -e-(b1-g)+fw, -w};
        uint32_t colorValues[] = {RED,  YELLOW, GREEN};
        if  (angle == 0)
        {
                for (int j=0;j<sizeof(xValues)/sizeof(xValues[0]);j++)
                {
                        for (double i=-c1/2-d1/2;i<-c1/2-d1/2+hw;i++)
                        {
                                double vecValue[] = {xValues[j], 0, isMirror * i, 1};
                                Vector *vector = InitVector(vector, 4);
                                ValueVector(vector, vecValue);
                                Vector *vecScreen = MatrixTransform(vector);
                                if (IsScreenPoint(vecScreen))
                                {
                                        SetVectorPixelOS(vecScreen, colorValues[j]);
                                }
                                FreeVector(vecScreen);
                                FreeVector(vector);
                        }
                        for (double i=f/2-hw;i<f/2 - hw + hw;i++)
                        {
                                double vecValue[] = {xValues[j], 0, isMirror * i, 1};
                                Vector *vector = InitVector(vector, 4);
                                ValueVector(vector, vecValue);
                                // printf("qianlunneiceValue %d: %lf\n", i, qianlunneiceValue);
                                Vector *vecScreen = MatrixTransform(vector);
                                if (IsScreenPoint(vecScreen))
                                {
                                        SetVectorPixelOS(vecScreen, colorValues[j]);
                                }
                                FreeVector(vecScreen);
                                FreeVector(vector);
                        }
                }
        }
        else
        {
                for (int j=0;j<sizeof(xValues)/sizeof(xValues[0]);j++)
                {
                        {       //每条横线中间隔开,所以又四个点,求出4个点在旋转Y度后的世界坐标
                                double vecValue0[] = {xValues[j], 0, isMirror * (-c1/2-d1/2), 1};
                                Vector *vector0 = InitVector(vector0, 4);
                                ValueVector(vector0, vecValue0);
                                double vecValue1[] = {xValues[j], 0, isMirror * (f/2), 1};
                                Vector *vector1 = InitVector(vector1, 4);
                                ValueVector(vector1, vecValue1);
                                Vector *pos0 = MatrixTransformHorizontalLine(vector0, isMirror * angle);
                                Vector *pos1 = MatrixTransformHorizontalLine(vector1, isMirror * angle);

                                double vecValue2[] = {xValues[j], 0, isMirror * (-c1/2-d1/2+hw), 1};
                                Vector *vector2 = InitVector(vector2, 4);
                                ValueVector(vector2, vecValue2);
                                double vecValue3[] = {xValues[j], 0, isMirror * (f/2-hw), 1};
                                Vector *vector3 = InitVector(vector3, 4);
                                ValueVector(vector3, vecValue3);
                                Vector *pos2 = MatrixTransformHorizontalLine(vector2, isMirror * angle);
                                Vector *pos3 = MatrixTransformHorizontalLine(vector3, isMirror * angle);

                                //求最上面和最下面的两个点的直线方程,其中斜率需要修正
                                double k = (pos0->data[2] - pos1->data[2]) / (pos0->data[0] - pos1->data[0]) + GetOffsetK(angle);
                                double c = pos1->data[2] - pos1->data[0] * k;

                                //求直线分别与铲外侧轨迹方程、前轮内侧轨迹方程的交点,经测试如果大于30度则与前轮内侧可能没有交点,这时使用铲内侧轨迹方程计算
                                Vector *pos4 = GetPointLineAndCircle(k, c, g + b2, -GetR2(angle) * isMirror, GetR1(angle) - c1 / 2 - d1 / 2);
                                // if ((pos4->data[0] != pos4->data[0]) ||  (pos4->data[1] != pos4->data[1]) ||  (pos4->data[2] != pos4->data[2]) ||  (pos4->data[3] != pos4->data[3]))
                                if (isnan(pos4->data[0]) || isnan(pos4->data[1]) || isnan(pos4->data[2]) || isnan(pos4->data[3]))
                                {
                                        pos4 = GetPointLineAndCircle(k, c, g + b2, -GetR2(angle) * isMirror, sqrt(pow((GetR1(angle) - f / 2), 2) + pow(e, 2)));
                                }
                                Vector *pos5 = GetPointLineAndCircle(k, c, g + b2, -GetR2(angle) * isMirror, sqrt(pow((GetR1(angle) + f / 2), 2) + pow(e, 2)));

                                //使用与轨迹求出的交点画线,并在中间断开一截
                                double len = fabs(pos4->data[0] - pos5->data[0]);
                                for (double i=(pos4->data[0] < pos5->data[0] ? pos4->data[0] : pos5->data[0]);i<=(pos4->data[0] > pos5->data[0] ? pos4->data[0] : pos5->data[0]);i+=len/50)
                                {
                                        if (i < (pos2->data[0] > pos3->data[0] ? pos2->data[0] : pos3->data[0]) && i > (pos2->data[0] < pos3->data[0] ? pos2->data[0] : pos3->data[0]))
                                        {
                                                continue;
                                        }
                                        double vecValue[] = {i, 0, k * i + c, 1};
                                        Vector *vector = InitVector(vector, 4);
                                        ValueVector(vector, vecValue);
                                        Vector *vecScreen = MatrixTransform(vector);
                                        if (IsScreenPoint(vecScreen))
                                        {
                                                SetVectorPixelOS(vecScreen, colorValues[j]);
                                        }
                                        FreeVector(vector);
                                        FreeVector(vecScreen);
                                }
                                FreeVector(pos5);
                                FreeVector(pos4);
                                FreeVector(pos3);
                                FreeVector(pos2);
                                FreeVector(pos1);
                                FreeVector(pos0);
                                FreeVector(vector3);
                                FreeVector(vector2);
                                FreeVector(vector1);
                                FreeVector(vector0);
                        }
                        // {       //画右侧横线
                        //         double vecValue0[] = {xValues[j], 0, isMirror * (f/2 - hw), 1};
                        //         Vector *vector0 = InitVector(vector0, 4);
                        //         ValueVector(vector0, vecValue0);
                        //         double vecValue1[] = {xValues[j], 0, isMirror * (f/2 - hw + hw), 1};
                        //         Vector *vector1 = InitVector(vector1, 4);
                        //         ValueVector(vector1, vecValue1);
                        //         Vector *pos0 = MatrixTransformHorizontalLine(vector0, isMirror * angle);
                        //         Vector *pos1 = MatrixTransformHorizontalLine(vector1, isMirror * angle);

                        //         double k = (pos0->data[2] - pos1->data[2]) / (pos0->data[0] - pos1->data[0]);
                        //         double c = pos0->data[2] - pos0->data[0] * k;

                        //         Vector *pos2 = GetPointLineAndCircle(k, c, g + b2, -GetR2(angle) * isMirror, sqrt(pow((f / 2 + GetR1(angle)), 2) + pow(e, 2)));

                        //         double len = fabs(pos0->data[0] - pos2->data[0]);
                        //         for (double i=(pos0->data[0] < pos2->data[0] ? pos0->data[0] : pos2->data[0]);i<=(pos0->data[0] > pos2->data[0] ? pos0->data[0] : pos2->data[0]);i+=len/20)
                        //         {
                        //                 double vecValue[] = {i, 0, k * i + c, 1};
                        //                 Vector *vector = InitVector(vector, 4);
                        //                 ValueVector(vector, vecValue);
                        //                 Vector *vecScreen = MatrixTransform(vector);
                        //                 if (IsScreenPoint(vecScreen))
                        //                 {
                        //                         SetVectorPixelOS(vecScreen, colorValues[j]);
                        //                 }
                        //                 FreeVector(vector);
                        //                 FreeVector(vecScreen);
                        //         }
                        //         FreeVector(pos2);
                        //         FreeVector(pos1);
                        //         FreeVector(pos0);
                        //         FreeVector(vector1);
                        //         FreeVector(vector0);
                        // }
                }
        }
}

void Trajectory(double w0)
{
        InitParamMatrix(w0);
        DrawTrajectory(w0);
        DrawHorizontalLine(w0);
        FreeParamMatrix();
}



int main(int argc, char *argv[])
{
	uint32_t *fb_base0;
	uint32_t *fb_base1;
	uint32_t *fb_ctrl;
	int i;
	int alpha;
 	int logo_fd;
	char disp_str[128];
	struct display_param param;
	int frame0;
	int fd_param;
	int *buffer_param;
	int fd_video;
	int *buffer_video;

	char freq[10];
	char dbm1[5];
	char dbm2[5];
	int flag_nosignal;
	int no_signal = 0;

	int freq_value;	

	int x0;
	int y0;

	int n_read;
	unsigned char _rx_data[16];
	int angel;
	float angel_f = 0.0;
	float angel_f_pre = 0.0;

	fb_base0 = (int32_t *)p2v(0x11800000, 1920*1080*4);
	printf("fb_base virt 0x%08x\n", (uint32_t)fb_base0);
	alpha = 65535;

	memset(fb_base0,0x00,1920*1080*4);
	fb_base1 = (int32_t *)p2v(0x12100000, 1920*1080*4);
        printf("fb_base virt 0x%08x\n", (uint32_t)fb_base1);
	
	memset(fb_base1,0x00,1920*1080*4);
	fb_ctrl = (int32_t *)p2v(0x43c00000, 4096);

	double W0=atof(argv[2]);

	i = 0;

	 n_read = uart_init(ROUTE_UART, "/dev/ttyUL0", 115200, 'N');
        if(n_read <=0 ) {
                printf("open /dev/ttyUL0 failed\n");
                return -1;
        }


          n_read = uart_get(ROUTE_UART, _rx_data);

	


	fd_video = shm_open("/video.txt",O_RDWR|O_CREAT|O_EXCL,0777);
        if(fd_video < 0){
               fd_video = shm_open("/video.txt",O_RDWR,0777);
        }else
                ftruncate(fd_video,4096);
         buffer_video= mmap(NULL,4096,PROT_READ|PROT_WRITE,MAP_SHARED,fd_video,0);


	 fb_ctrl[56] = 0x5;
	

	 working_fb=fb_base0;


        InitParameter();

	while(1) {
	

		flag_nosignal = buffer_video[0];


		n_read = uart_get(ROUTE_UART, _rx_data);
		if(n_read == 8){
			angel = ((_rx_data[6] & 0x7) ) | (_rx_data[7]<<3) ;
			angel_f = ((float)(angel-900))*0.1;
			printf("angel_f %f\n",angel_f);
		}

	#if 1	
		if((flag_nosignal==0) && (no_signal ==0)){
			logo_fd = open("/tmp/nosignal.bmp", O_RDONLY, S_IRUSR);
			fill_bmp(logo_fd, (uint32_t *)(fb_base0), alpha, 1920, 1080);
			close(logo_fd);
			no_signal = 1;
		}else if((flag_nosignal == 1) && (no_signal == 1)){
			blt_clr(fb_base0,0,0,1920,1080);
			no_signal =0 ;

			
		}
		#if 1 
		// if(no_signal == 0){
                if(no_signal == 0 && (angel_f != angel_f_pre)){
                        blt_clr(fb_base0,640,320,1280,800);
			angel_f_pre = angel_f;

			// render(angel_f,640+190+240,700,1);
        		// render(angel_f,640+170,700,0);
                        Trajectory(angel_f);

		}
		#endif


	#endif
		usleep(2000);

	}

        FreeParameter();

	return 0;
}
